"""
来自论文[1] 许利恒.基于椭圆特征的陨石坑检测与识别方法研究[D].北京:北京航空航天大学,2022

按照论文的方法复现，关于从四点金字塔拓展至整幅陨石坑的算法，这里使用了姿态解算的结果作为重投影平差的依据，不再使用导航坑对的不变量计算，增加了对陨石坑的筛选算法
"""

import numpy as np
from .base import BinadFastPyramid


class XuFastPyramid(BinadFastPyramid):

    def binad_descriptor(self, C1, C2, *args, factor=0.3, **kwargs):
        """
        本函数需要并行化计算
        Arguments:
            param1 (np.ndarray) : ellipse parameters for (x^2, xy, y^2, x, y, 1), 6 x N array
            param2 (np.ndarray) : ellipse parameters for (x^2, xy, y^2, x, y, 1), 6 x N array
        """
        # 直接除以行列式是不对滴，参见文献
        # [1] Gros P. , Quan L. .Projective invariants for Vision[R].1992:47
        # from Xuliheng' methon
        if len(C1.shape) == 2:
            C1 = C1[None]
        if len(C2.shape) == 2:
            C2 = C2[None]
        assert C1.shape == C2.shape
        det = np.linalg.det(C1) / np.linalg.det(C2)
        I1 = np.trace(C1 @ np.linalg.inv(C2), axis1=1, axis2=2) ** 3 / det
        I2 = np.trace(C2 @ np.linalg.inv(C1), axis1=1, axis2=2) ** 3 * det
        I = np.array((I1, I2))
        return I, factor * np.abs(I)
